/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-03-08     Hao       the first version
 */
#include "bldc_motor.h"
#include <math.h>

BLDCMotor::BLDCMotor(int pp)
{
    // 极对数
    pole_pairs_ = pp;

    // 电机状态定义
    enabled_ = 0;

    // 磁传感器
    zero_electric_angle_ = NOT_SET;     // 传感器0度时电机的电角度
    encoder_direction_ = UNKNOWN;       // 传感器方向

    // 控制模式
    torque_controller_ = FOC_CURRENT; // 扭矩控制模式
    motion_controller_ = TORQUE;      // 应用控制模式

    // 控制目标初始化全部赋值为0
    target_ = 0;
    current_sp_ = 0;
    shaft_velocity_sp_ = 0;
    shaft_angle_sp_ = 0;
    voltage_.d = 0;
    voltage_.q = 0;
    current_.q = 0;
    current_.d = 0;

    // 控制过程限制参数
    velocity_limit_ = DEF_VEL_LIM;                  // for 位置 mode
    voltage_limit_ = DEF_POWER_SUPPLY / _SQRT3;     // for 电流 mode
    current_limit_ = DEF_CURRENT_LIM;               // for 速度 mode

    // 对齐传感器用的电压
    voltage_encoder_align_ = DEF_VOLTAGE_ENCODER_ALIGN;

    // 角度计算相关
    shaft_angle_ = 0;
    electrical_angle_ = 0;
    shaft_velocity_ = 0;

    SIMPLE_DEBUG("MOTOR:motor_uninitialized.\n");
}

BLDCMotor::~BLDCMotor()
{
}

// 初始化
void BLDCMotor::init()
{
    if (!driver_)
    {
        SIMPLE_DEBUG("MOTOR:motor_init_failed.\n");
        return;
    }
    SIMPLE_DEBUG("MOTOR:motor_initializing.\n");

    // 检查电压限制和驱动的电压限制
    if (voltage_limit_ > driver_->voltage_limit_)
        voltage_limit_ = driver_->voltage_limit_;
    // 对齐电机传感器时用的电压
    if (voltage_encoder_align_ > voltage_limit_)
        voltage_encoder_align_ = voltage_limit_;

    // 电流模式下的限制
    if (current_sense_)
    {
        // 电流环限制最大电压
        PID_current_q_.limit_ = voltage_limit_;
        PID_current_d_.limit_ = voltage_limit_;
    }

    // 速度模式下的限制
    if (torque_controller_ != VOLTAGE)  // 如果使用电流控制，限制最大电流
    {
        PID_velocity_.limit_ = current_limit_;  // 速度模式下限制电流
    }
    else    // 如果使用电压控制，限制最大电压
    {
        PID_velocity_.limit_ = voltage_limit_;  // 速度模式下限制电压
    }

    // 位置模式下的速度限制
    P_angle_.limit_ = velocity_limit_;

    // 开环控制，或者方向未设置，默认为CW
    if ((motion_controller_ == ANGLE_OPENLOOP || motion_controller_ == VELOCITY_OPENLOOP)
            && (encoder_direction_ == UNKNOWN))
    {
        encoder_direction_ = CW;
    }

    // 使能电机
    enable();

    SIMPLE_DEBUG("MOTOR:motor_uncalibrated.\n");
}

// 使能电机
void BLDCMotor::enable()
{
    driver_->enable();
    driver_->setPwm(0, 0, 0);
    enabled_ = 1;
}

// 失能电机
void BLDCMotor::disable()
{
    driver_->setPwm(0, 0, 0);
    driver_->disable();
    enabled_ = 0;
}

// 绑定磁传感器
void BLDCMotor::linkEncoder(Encoder *encoder)
{
    encoder_ = encoder;
}

// 绑定电流传感器
void BLDCMotor::linkCurrentSense(InlineCurrentSense *current_sense)
{
    current_sense_ = current_sense;
}

// 绑定电机驱动器
void BLDCMotor::linkDriver(Driver *driver)
{
    driver_ = driver;
}

// 获取轴角度 shaft angle calculation
float BLDCMotor::shaftAngle()
{
    // if no encoder_ linked return previous value ( for open loop )
    if (!encoder_)
        return shaft_angle_;

    return encoder_direction_ * LPF_angle_(encoder_->getAngle());
}

// 获取轴速度 shaft velocity calculation
float BLDCMotor::shaftVelocity()
{
    // if no encoder_ linked return previous value ( for open loop )
    if (!encoder_)
        return shaft_velocity_;
    return encoder_direction_ * LPF_velocity_(encoder_->getVelocity());
}

// 计算电角度
float BLDCMotor::electricalAngle()
{
    // if no encoder_ linked return previous value ( for open loop )
    if (!encoder_)
        return electrical_angle_;
    return _normalizeAngle(
            (float) (encoder_direction_ * pole_pairs_) * encoder_->getMechanicalAngle() - zero_electric_angle_);
}

// foc初始化，校准编码器
int BLDCMotor::initFOC()
{
    SIMPLE_DEBUG("MOTOR:motor_calibrating.\n");

    delayMillisecond(100);

    // 校准编码器
    if (encoder_)
    {
        if (1 == alignEncoder())
        {
            SIMPLE_DEBUG("MOTOR:motor_ready.\n");
        }
        else
        {
            SIMPLE_DEBUG("MOTOR:motor_calib_failed.\n");
            disable();
            return 0;
        }
        // 更新轴角度
        encoder_->update();
        shaft_angle_ = shaftAngle();
    }
    else
    {
        SIMPLE_DEBUG("MOTOR:No sensor.\n");
        SIMPLE_DEBUG("MOTOR:motor_calib_failed.\n");

        return 0;
    }

    return 1;
}

// 检测传感器方向、检验极对数、计算编码器偏差
// 成功返回1，失败返回0
int BLDCMotor::alignEncoder()
{
    SIMPLE_DEBUG("MOTOR:Align sensor.\n");

    // 如果未设置传感器方向
    if (encoder_direction_ == UNKNOWN)
    {
        // 正转，记录最后的角度
        for (int i = 0; i <= 500; i++)
        {
            float angle = _3PI_2 + _2PI * i / 500.0f;
            setPhaseVoltage(voltage_encoder_align_, 0, angle);
            encoder_->update();
            delayMillisecond(2);
        }
        encoder_->update();
        float mid_angle = encoder_->getAngle();

        // 反转，记录最后的角度
        for (int i = 500; i >= 0; i--)
        {
            float angle = _3PI_2 + _2PI * i / 500.0f;
            setPhaseVoltage(voltage_encoder_align_, 0, angle);
            encoder_->update();
            delayMillisecond(2);
        }
        encoder_->update();
        float end_angle = encoder_->getAngle();

        // 停止输出
        setPhaseVoltage(0, 0, 0);
        delayMillisecond(200);

        // 计算方向
        float moved = fabs(mid_angle - end_angle);
        if (moved < 0.01)   // 几乎没有动
        {
            SIMPLE_DEBUG("MOTOR:Failed to notice movement.\n");
            return 0;
        }
        else if (mid_angle < end_angle)
        {
            SIMPLE_DEBUG("MOTOR:sensor_direction==CCW.\n");
            encoder_direction_ = CCW;
        }
        else
        {
            SIMPLE_DEBUG("MOTOR:sensor_direction==CW.\n");
            encoder_direction_ = CW;
        }

        // 检查极对数
        if (fabs(moved * pole_pairs_ - _2PI) > 0.5f)
        {
            SIMPLE_DEBUG("MOTOR:PP check: fail - estimated pp: %d.\n", (int)(_2PI / moved));
        }
        else
        {
            SIMPLE_DEBUG("MOTOR:PP check: OK!\n");
        }
    }
    else
    {
        SIMPLE_DEBUG("MOTOR:Skip dir calib.\n");
    }

    // 如果未设置电角度0度时的传感器角度
    if (!_isset(zero_electric_angle_))
    {
        // 设置电机转到0度
        setPhaseVoltage(voltage_encoder_align_, 0, _3PI_2);
        delayMillisecond(500);
        // 读取编码器值
        encoder_->update();
        // 读取当前电角度
        zero_electric_angle_ = 0;
        zero_electric_angle_ = electricalAngle();
        delayMillisecond(20);


        SIMPLE_DEBUG("MOTOR:Zero elec. angle: %d.%d .\n", (int)(zero_electric_angle_ * 1000) / 1000, (int)(zero_electric_angle_ * 1000) % 1000);

        // 停止输出
        setPhaseVoltage(0, 0, 0);
        delayMillisecond(200);
    }
    else
    {
        SIMPLE_DEBUG("MOTOR:Skip offset calib.\n");
    }

    return 1;
}

// 外环控制
// 1. 如果是电压模式，就直接计算最后的目标电压，由focloop函数直接下发。
// 2. 如果是电流模式，则计算外环最终需要的目标电流，交给focloop函数计算最终的目标电压然后下发。
void BLDCMotor::move(float new_target)
{
    // 获取轴角度、轴速度，开环模式下会自动更新
    if (motion_controller_ != ANGLE_OPENLOOP && motion_controller_ != VELOCITY_OPENLOOP)
    {
        shaft_angle_ = shaftAngle();
        shaft_velocity_ = shaftVelocity();
    }

    if (!enabled_)
        return;

    // set internal target variable
    if (_isset(new_target))
        target_ = new_target;

    // upgrade the current based voltage limit
    switch (motion_controller_)
    {
    case TORQUE:
        if (torque_controller_ == VOLTAGE)
        {
            // 如果电压控制，直接把目标值作为 voltage_.q 最后下发即可
            voltage_.q = target_;
            voltage_.q = _constrain(voltage_.q, -voltage_limit_, voltage_limit_);
            voltage_.d = 0;
        }
        else
        {
            // 电流控制模式下，则交给电流环处理，计算需要的 voltage
            current_sp_ = target_;
        }
        break;
    case ANGLE:
        shaft_angle_sp_ = target_;
        // 计算速度输入
        shaft_velocity_sp_ = P_angle_(shaft_angle_sp_ - shaft_angle_);
        shaft_angle_sp_ = _constrain(shaft_angle_sp_, -velocity_limit_, velocity_limit_);
        // 计算电流输入
        current_sp_ = PID_velocity_(shaft_velocity_sp_ - shaft_velocity_);
        // 如果电压控制，直接把目标值作为 voltage_.q 最后下发即可
        if (torque_controller_ == VOLTAGE)
        {
            voltage_.q = current_sp_;
            voltage_.d = 0;
        }
        break;
    case VELOCITY:
        shaft_velocity_sp_ = target_;
        // 计算电流输入
        current_sp_ = PID_velocity_(shaft_velocity_sp_ - shaft_velocity_);
        // 如果电压控制，直接把目标值作为 voltage_.q 最后下发即可
        if (torque_controller_ == VOLTAGE)
        {
            voltage_.q = current_sp_;
            voltage_.d = 0;
        }
        break;
    case VELOCITY_OPENLOOP:
        shaft_velocity_sp_ = target_;
        voltage_.q = velocityOpenloop(shaft_velocity_sp_); // returns the voltage_ that is set to the motor
        voltage_.d = 0;
        break;
    case ANGLE_OPENLOOP:
        shaft_angle_sp_ = target_;
        voltage_.q = angleOpenloop(shaft_angle_sp_); // returns the voltage_ that is set to the motor
        voltage_.d = 0;
        break;
    }
}

// 力矩换循环，运行频率越高越好
// 1. 如果是电压模式，则直接下发目标电压
// 2. 如果是电流模式，则根据电流目标值计算目标电压
void BLDCMotor::loopFOC()
{
    // 更新编码器数据
    if (encoder_)
        encoder_->update();

    // 开环运行无需力矩环
    if (motion_controller_ == ANGLE_OPENLOOP || motion_controller_ == VELOCITY_OPENLOOP)
    {
        return;
    }

    // 未使能
    if (!enabled_)
        return;

    // 获取电角度
    electrical_angle_ = electricalAngle();
    switch (torque_controller_)
    {
    case VOLTAGE:
        // 无需计算电流
        break;
    case DC_CURRENT:
        if (!current_sense_)
            return;
        // 获取电流
        current_.q = current_sense_->getDCCurrent(electrical_angle_); // read overall current magnitude
        current_.q = LPF_current_q_(current_.q);                      // filter the value values
        // 计算相电压，只计算q向量
        voltage_.q = PID_current_q_(current_sp_ - current_.q);  // calculate the phase voltage_
        voltage_.d = 0;                                         // d voltage_  - lag compensation
        break;
    case FOC_CURRENT:
        if (!current_sense_)
            return;
        // 获取电流
        current_ = current_sense_->getFOCCurrents(electrical_angle_); // read dq currents
        current_.q = LPF_current_q_(current_.q);                      // filter values
        current_.d = LPF_current_d_(current_.d);
        // 计算相电压，d、q都需要
        voltage_.q = PID_current_q_(current_sp_ - current_.q);
        voltage_.d = PID_current_d_(0 - current_.d);
        break;
    default:
        SIMPLE_DEBUG("MOTOR:no torque control selected!\n");
        break;
    }

    setPhaseVoltage(voltage_.q, voltage_.d, electrical_angle_);
}

// Method using FOC to set Uq and Ud to the motor at the optimal angle
// Function implementing Space Vector PWM and Sine PWM algorithms
//
// Function using sine approximation
// regular sin + cos ~300us    (no memory usage)
// approx  _sin + _cos ~110us  (400Byte ~ 20% of memory)
void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el)
{
    uint32_t sector;
    float Uout;

    // 计算目标电压大小和力矩电角度
    if (Ud)
    {
        Uout = _sqrtApprox(Ud * Ud + Uq * Uq) / driver_->voltage_limit_;
        angle_el = _normalizeAngle(angle_el + atan2(Uq, Ud));
    }
    else
    {
        Uout = Uq / driver_->voltage_limit_;
        angle_el = _normalizeAngle(angle_el + _PI_2);
    }

    // 计算当前扇区
    sector = (angle_el / _PI_3) + 1;

    // 计算三个电压向量的占空比
    float T1 = _SQRT3 * _sin(sector * _PI_3 - angle_el) * Uout;
    float T2 = _SQRT3 * _sin(angle_el - (sector - 1.0f) * _PI_3) * Uout;
    float T0 = 1 - T1 - T2;

    // 计算三个电压向量持续时间（SVPWM）
    float Ta, Tb, Tc;
    switch (sector)
    {
    case 1:
        Ta = T1 + T2 + T0 / 2;
        Tb = T2 + T0 / 2;
        Tc = T0 / 2;
        break;
    case 2:
        Ta = T1 + T0 / 2;
        Tb = T1 + T2 + T0 / 2;
        Tc = T0 / 2;
        break;
    case 3:
        Ta = T0 / 2;
        Tb = T1 + T2 + T0 / 2;
        Tc = T2 + T0 / 2;
        break;
    case 4:
        Ta = T0 / 2;
        Tb = T1 + T0 / 2;
        Tc = T1 + T2 + T0 / 2;
        break;
    case 5:
        Ta = T2 + T0 / 2;
        Tb = T0 / 2;
        Tc = T1 + T2 + T0 / 2;
        break;
    case 6:
        Ta = T1 + T2 + T0 / 2;
        Tb = T0 / 2;
        Tc = T1 + T0 / 2;
        break;
    default:
        Ta = 0;
        Tb = 0;
        Tc = 0;
    }

    // 计算下发给驱动器的相电压
    float Ua = Ta * driver_->voltage_limit_;
    float Ub = Tb * driver_->voltage_limit_;
    float Uc = Tc * driver_->voltage_limit_;

    // 下发到驱动器
    driver_->setPwm(Ua, Ub, Uc);
}

// 速度开环控制
// - target_velocity - rad/s
float BLDCMotor::velocityOpenloop(float target_velocity)
{
    // 计算时间差
    unsigned long now_us = micros();
    float Ts = (now_us - open_loop_timestamp_) * 1e-6f;
    if (Ts <= 0 || Ts > 0.5f)
        Ts = 1e-3f;

    rt_kprintf("Ts=%d.%d \n", (int)(Ts * 1000) / 1000, (int)(Ts * 1000) % 1000);

    // 计算达到目标速度所需的角度
    shaft_angle_ = _normalizeAngle(shaft_angle_ + target_velocity * Ts);
    // for display purposes
    shaft_velocity_ = target_velocity;

    // 开环模式下使用固定的电压
    float Uq = voltage_limit_;

    // 直接下发电压，无需经过loopfoc
    setPhaseVoltage(Uq, 0, _electricalAngle(shaft_angle_, pole_pairs_));

    // 更新时间参数
    open_loop_timestamp_ = now_us;

    return Uq;
}

// 角度开环控制
// - target_velocity - rad
float BLDCMotor::angleOpenloop(float target_angle)
{
    unsigned long now_us = micros();
    float Ts = (now_us - open_loop_timestamp_) * 1e-6f;
    if (Ts <= 0 || Ts > 0.5f)
        Ts = 1e-3f;

    // 计算当前位置移动到目标位置需要的速度
    if (_abs(target_angle - shaft_angle_) > _abs(velocity_limit_ * Ts))
    {
        shaft_angle_ += _sign(target_angle - shaft_angle_) * _abs(velocity_limit_) * Ts;
        shaft_velocity_ = velocity_limit_;
    }
    else
    {
        shaft_angle_ = target_angle;
        shaft_velocity_ = 0;
    }

    // 开环模式下使用固定的电压
    float Uq = voltage_limit_;

    // 直接下发电压，无需经过loopfoc
    setPhaseVoltage(Uq, 0, _electricalAngle(_normalizeAngle(shaft_angle_), pole_pairs_));

    // 更新时间参数
    open_loop_timestamp_ = now_us;

    return Uq;
}

